import time
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix
from geographiclib.geodesic import Geodesic
apmControllernNameSpace='/apm_drone'
class GPSPublisherNode(Node):
    def __init__(self):
        super().__init__('gps_publisher_node')

        # 创建NavSatFix消息
        msg = NavSatFix()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'world'

        # 1 -35.3618886 149.1655396
        # 2 -35.3631518 149.1672889
        # 3 -35.3618624 149.1669339

        # msg.latitude = -35.3632616 #0
        # msg.longitude = 149.1652381

        # msg.latitude = -35.3618886
        # msg.longitude = 149.1655396

        # msg.latitude = -35.3631518
        # msg.longitude = 149.1672889
        
        msg.latitude = 30.3098891
        msg.longitude = 120.3847906


        msg.altitude = 100.0

        # 创建发布者
        self.publisher_ = self.create_publisher(NavSatFix, apmControllernNameSpace+'/target_gps_location', 10)
        time.sleep(0.5)
        while True:
            # 发布消息
            self.publisher_.publish(msg)
            self.get_logger().info('Published target GPS location: lat={}, lon={}, alt={}'.format(msg.latitude, msg.longitude, msg.altitude))
            time.sleep(1.0)

    def dms_to_dd(self,degrees, minutes, seconds):
        return degrees + minutes / 60 + seconds / 3600

def main(args=None):
    rclpy.init(args=args)
    node = GPSPublisherNode()
    rclpy.spin_once(node, timeout_sec=0)  # 只执行一次
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
